/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!  \file dataDistribution.cxx
  \brief class implemenation XML parser

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <socket.h>
#include <arpa/inet.h>

#include "dataDistribution.h"
#include "color.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
  /*! \brief determines level of output to user. */
  extern bool verbose;
  extern bool logging;

  extern in_addr netmask;
  extern in_addr internal_subnet;
/*<-End Global Variables-->*/

// these are used for cout
using std::hex;
using std::dec;

Data_Distribution::Data_Distribution(String name, String logDir) 
	: ThreadedPipe(name), comm_log(logDir.c_str(), FileLog::CREATE) 
{
	num_interfaces=0;

	transmit_cap = false;
	last_cap_tx.stamp();
	last_cap_tx -= long((CAPABILITES_PERIOD-1) * SEC2MICRO);
}

/************************************************************\
  |               Data_Distribution::~Data_Distribution        |
  \************************************************************/
/*!
  \brief closes socket connection and inetRxFifo
  \pre instance of InetRx_Socket has been created and initialized
  \post socket connections have been closed and inetRxFifo has been closed and deleted 
  */
Data_Distribution::~Data_Distribution() 
{
	if(logging) {
		if ( ! comm_log.close() ) {
			cout << "Could not close" << comm_log.getFileName() << endl;
			perror("comm_log.close()");
		}   
	}
}

void Data_Distribution::registerInterface(Interface * an_interface)
{
	if( ! an_interface ) return;
	interfaces[num_interfaces].pointer = an_interface;

	interfaces[num_interfaces].RxFD = -1;
	interfaces[num_interfaces].TxFD = -1;

	String a = an_interface->getFifo(Interface::RX);

	if(interfaces[num_interfaces].pointer->getFifo(Interface::RX) != "") {
		if( (interfaces[num_interfaces].RxFD = open(interfaces[num_interfaces].pointer->getFifo(Interface::RX).c_str(), O_WRONLY )) <= 0) { 
			perror("Data_Distribution::init() - could not open named fifo");
		}
	}
	if(interfaces[num_interfaces].pointer->getFifo(Interface::TX) != "") {
		if( (interfaces[num_interfaces].TxFD = open(interfaces[num_interfaces].pointer->getFifo(Interface::TX).c_str(), O_RDONLY )) <= 0) { 
			perror("Data_Distribution::init() - could not open named fifo");
		}
	}

	interfaces[num_interfaces].pointer->registerCapabilities(shmPtr);
	num_interfaces++;
}

/************************************************************\
|                 Data_Distribution::update                   |
\************************************************************/
/*!
  \brief function called periodically by the thread.
  \pre instance of Data_Distribution has been created and initialized
  \post any data in the inetRxFifo will be parsed and output in
  command format to the SAVTxFifo
*/
void Data_Distribution::update() 
{
	if(logging) {
		if ( !comm_log.isOpen() ) {
			if ( !comm_log.open() ) {
				cout << "Could not open " << comm_log.getFileName() << endl;
				perror("comm_log.open()");
			}
		}
	}

	// check time stamps and update status
	currentStamp.stamp();
	if((currentStamp - last_cap_tx ) > (CAPABILITES_PERIOD * SEC2MICRO) ) {
		transmit_cap = true;
	}

	// now update client status 
	Client * a_client;
	int c_ind = 1;
	while( (a_client = shmPtr->clients.getClient(c_ind)) ) 
	{
		// if we missed 2 capabilities packets, then the nodes has timed out
		if( (currentStamp - a_client->last_rx) > (4.5 * CAPABILITES_PERIOD * SEC2MICRO) )
		{
			if( a_client->state.timed_out == 0 ) 
				cout << RED << a_client->name << " -- timed out" << ATTROFF << endl;
			a_client->state.timed_out = 1;

			//-- at this time, we don't ever remove a client from our lists
			//shmPtr->clients.unsubscribe(a_client, 0);
			//shmPtr->clients.remove(a_client);

		} else if( a_client->state.timed_out ) {
			cout << RED << a_client->name << " -- client back" << ATTROFF << endl;

			// reset client state so we do not assume anything on the node
			a_client->state.timed_out = 0;
			a_client->state.gps       = 0;

			a_client->telemetry.navigation.APon   = FALSE;
			a_client->telemetry.navigation.MAmode = APMA_STANDARD;
			a_client->telemetry.navigation.APmode = AP_PRELAUNCH;
			a_client->telemetry.navigation.OBmode = OB_OFF;
			a_client->telemetry.navigation.OBtype = OB_NONE;
		}

		// get next client
		c_ind++;
	}

	// now see if there are any commands
	int val, maxFD = 0; fd_set 	readFDs, writeFDs;				// read fd set
	struct timeval timeout;

	FD_ZERO(&readFDs);				// zero read fd set
	FD_ZERO(&writeFDs);				// zero read fd set

	for(int i =0; i < num_interfaces; i++) {
		if(interfaces[i].TxFD >= 0)
			FD_SET(interfaces[i].TxFD, &readFDs);
		if(interfaces[i].RxFD >= 0)
			FD_SET(interfaces[i].RxFD, &writeFDs);
		maxFD = (interfaces[i].TxFD > maxFD ? interfaces[i].TxFD : maxFD);
		maxFD = (interfaces[i].RxFD > maxFD ? interfaces[i].RxFD : maxFD);
	}

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs, NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("Data_Distribution::update() select");
				execRate = -1;
			} else
				cout << "Data_Distribution::update() - select interrupted" << endl;
		}
	}
	else {

		// see if we need to transmit a capabilities message 
		if(transmit_cap)
		{
			if(verbose) 
				shmPtr->clients.printList();

			shmPtr->capabilities.getCapabilities((uint8_t*)fifo_msg.data);
			Client * me          = shmPtr->clients.getClient(0);
			fifo_msg.size        = CMD_SIZE(fifo_msg.data);

			// loop over network interfaces
			int if_count = 0;
			//cout << "if_count="<< if_count << " s_adder=" << inet_ntoa(me->address[if_count]) << endl;
			while( me->address[if_count].s_addr != INADDR_ANY ) {

				// if the address has our internal subnet for broadcasting, and the
				// interface has the appropriate subnet, then send the packet
				//cout << "if_addr="<< inet_ntoa(netmask) ;
				//cout << " netmask="<< inet_ntoa( me->address[if_count] ) ;
				//cout << " internal_subnet=" << inet_ntoa(internal_subnet) << endl;
				if( (me->address[if_count].s_addr & netmask.s_addr) == internal_subnet.s_addr) {
					fifo_msg.src.s_addr  = me->address[if_count].s_addr;
					fifo_msg.dest.s_addr = me->bcast_addr[if_count].s_addr;
					fifo_msg.port        = 1000;

					for(int j =0; j < num_interfaces; j++) {
						//cout << "interfaces[" << j << "].getSubnet() = " << inet_ntoa(interfaces[j].pointer->getSubnet()) << endl;
						if(interfaces[j].pointer->getSubnet().s_addr != INADDR_NONE) {
							if(interfaces[j].RxFD > 0 && FD_ISSET(interfaces[j].RxFD, &writeFDs)) {
								if(logging)
									comm_log.write(fifo_msg.src, fifo_msg.dest,  (char *)(fifo_msg.data), fifo_msg.size);
								writeFifo(interfaces[j].RxFD);
							}
						}
					}
					//cout << "<-- Sent CMD_CAPABILITY: dest=" << inet_ntoa(fifo_msg.dest) << " -->" << endl;
				}

				if_count++;
			}
			last_cap_tx.stamp();
			transmit_cap = false;
		}

		for(int i =0; i < num_interfaces; i++) {
			if(interfaces[i].TxFD > 0 && FD_ISSET(interfaces[i].TxFD, &readFDs)) {

				// get the bytes out of the fifo
				//cout << "readFifo(interfaces[i].TxFD) - " << endl;
				int n=readFifo(interfaces[i].TxFD);

				if(n>0) {

					// log data
					if(logging)
						comm_log.write(fifo_msg.src, fifo_msg.dest,  (char *)(fifo_msg.data), fifo_msg.size);

					//if((interfaces[i].pointer->getSubnet().s_addr == (address.s_addr & netmask.s_addr))) 
					//shmPtr->capabilities.printCapabilities();

					// do some error checking
					uint16_t length = CMD_SIZE(fifo_msg.data);
					if( length != fifo_msg.size) {
						cout << getName() << "::update() - error, length of command != length of buffer" << endl
							<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

						// if there is enough in the fifo, see if there is a valid command
						if( length <= fifo_msg.size ) {
							if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
								cout << "\tCMD: is valid" << endl;
							else {
								cout << "\tCMD: is NOT valid" << endl;
								continue;
							}
						} else {
							cout << "\tCMD is too large" << endl;
							continue;
						}
					}

					//-- form command and parse it out
					Command cmd((uint8_t*)fifo_msg.data);
					if(cmd.isValid()) 
					{
						parse(cmd, fifo_msg.src);

						//cout << endl << endl << "CMD: " << (int)cmd.cmd() << " [" << cmd.getName() << "] - TYPE=" << cmd.getType() << endl;
						//cout << "LOOP THROUGH INTERFACES" << endl;
						
						/*<------TX Interface----->*/
						for(int j=0; j < num_interfaces; j++) 
						{
							// don't forward to the same interface that got the message
							if(j != i)
							{
								//cout << "Interface : " << interfaces[j].pointer->getName() << endl;
								if( (interfaces[j].pointer->getSubnet().s_addr == (fifo_msg.dest.s_addr & netmask.s_addr)) 
									  || (interfaces[j].pointer->getSubnet().s_addr == INADDR_ANY) ) 
								{
									if(interfaces[j].RxFD > 0 && FD_ISSET(interfaces[j].RxFD, &writeFDs)) 
									{
										if((interfaces[j].pointer->getPort() == fifo_msg.port) || (interfaces[j].pointer->getPort() == 0)) 
										{

											Client * to_client = shmPtr->clients.getClient(fifo_msg.dest);
											Client * me = shmPtr->clients.getClient(0);

											if( (interfaces[j].pointer->hasExternalIF() && to_client != me) || interfaces[j].pointer->hasGateway()) {
												//cout << "FWD packet to EXTERNAL IF: " << interfaces[j].pointer->getName() << endl;
												//cout << "src addr=" << inet_ntoa(fifo_msg.src) << endl;
												//cout << "me addr=" << inet_ntoa( shmPtr->clients.getAddr(me, to_client)) << endl;

												// see if its a command, and if the interface is allowed to send out commands
												if( cmd.getType() == CMD_COMMAND && !interfaces[j].pointer->canCommand() )
													continue;

												writeFifo(interfaces[j].RxFD);
											//} else if( to_client == me && cmd.getType() != CMD_STREAM_REQUEST) {
											//} else if( to_client == me && !interfaces[j].pointer->hasExternalIF() ) {  FIXME -- this should be correct not the next line
											} else if( !interfaces[j].pointer->hasExternalIF() ) {
												//cout << "FWD packet to INTERNAL IF: " << interfaces[j].pointer->getName() << endl;
												if( cmd.getType() == CMD_COMMAND && !interfaces[j].pointer->canCommand() )
													continue;
												writeFifo(interfaces[j].RxFD);
											}

										} else {
											if(verbose) {
												cout << "Data_Distribution::update() - port " << fifo_msg.port << flush;
												cout << " does not match interface port " << inet_ntoa(interfaces[j].pointer->getSubnet()) 
													  <<  ":" << interfaces[j].pointer->getPort() << endl;
											}
										}
									} else { 
										cout << "Data_Distribution::update() - no rx fifo for IF " << j 
											  << ":" << inet_ntoa(interfaces[j].pointer->getSubnet()) << endl;
										cout << "Data_Distribution::update() - tx fifo name: " 
											  << interfaces[j].pointer->getFifo(Interface::TX) << endl;
									}
								} else {
									if(verbose) {
										cout << "Data_Distribution::update() - unrecognized subnet: " << flush;
										cout << inet_ntoa(interfaces[j].pointer->getSubnet()) << " : " << flush;
										cout << inet_ntoa(fifo_msg.dest) << " [" << flush;
										cout << inet_ntoa(netmask) << "]" <<endl;
										cout << interfaces[j].pointer->getName() << endl;
									}
								}
							}
						}
					} else {
						//if(varbose) 
						{
							cout << "-- INVALID COMMAND --" << endl
								<< "cmd.cmd() = " << hex << (int)cmd.cmd() << " : " << "DefinedCommand.id=" << (int)DefinedCommands[ cmd.cmd() ].id  << dec << endl
								<< "cmd.num() = " << (int)cmd.num() << " : " << "DefinedCommand.numParams=" << (int)DefinedCommands[ cmd.cmd() ].numParams << endl
								<< endl;
	
						}
					/*<------TX Interface----->*/
					}
				}
			}
		}
	}
}

void Data_Distribution::parse(const Command &cmd, in_addr address) 
{
	bool print = false;

	Client * a_client    = shmPtr->clients.getClient(fifo_msg.src);
	Client * to_client   = shmPtr->clients.getClient(fifo_msg.dest);
	Client * me          = shmPtr->clients.getClient(0);

	// if we know about the source client, then make sure the
	// message is to this node
	if( a_client ) {
		a_client->last_rx.stamp();

		if( to_client != me ) {
			//cout << "not to me: to=>" << a_client->name << endl;
			return;
		}
	}
	
	//cout << "Data_Distribution::parse -- " << " COMMAND [" << hex << (int)cmd.cmd() << dec << "] from [" 
	//<< inet_ntoa(address) << "]" << endl;

	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

	mutex_lock();

	switch(cmd.cmd()) {

		//----------------------------------------------------------
		// Handle the action of subscriptions and capabilities
		//----------------------------------------------------------
		case CMD_CAPABILITY: {
			SystemCharacter_t * a_system;

			if(a_client == NULL) { 
				String name = ((VehicleInfo_t*)dataPtr)->id;
				cout << "Adding new client: " YELLOW << name << ATTROFF
					<< " [" CYAN << inet_ntoa(fifo_msg.src) << ATTROFF "]" << endl;

				shmPtr->clients.add(name);
				a_client = shmPtr->clients.getClient(name);
				if( a_client ) {
					shmPtr->clients.addAddr(a_client, fifo_msg.src);
					a_client->last_rx.stamp();
					print = true;
				} else  {
					cout << "Data_Distribution::parse - could not get newly created client" << endl;
				}
			}

			if( a_client == NULL ) {
				cout << "Data_Distribution::parse - could not get a valid client" << endl;
				break;
			}

			// remove previous capabilites
			a_client->capabilities.remove(NULL); 

			// fill in new ones
			dataPtr += sizeof(VehicleInfo_t);
			int nSys = ((cmd.num()-sizeof(VehicleInfo_t))/sizeof(SystemCharacter_t));
			for(int i=0; i < nSys; i++) {
				a_system = &(((SystemCharacter_t*)dataPtr)[i]);
				a_client->capabilities.add(a_system);
			}

			if(verbose & print)
				shmPtr->clients.printList();
			break;
		}

		case CMD_STREAM_REQ: {
			StreamReq_t * request = (StreamReq_t *)dataPtr;

			if( verbose )
			cout << "Stream Request [" << hex << (int)request->id << dec << "] from "
				<< inet_ntoa(request->client) << " on port "
				<< request->port << " with action "
				<< request->send_data << endl;

			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}

			if(request->send_data) {
				//if(verbose)
				cout << GREEN << me->name << ATTROFF " -- subscribing " YELLOW << a_client->name 
				     << ATTROFF ":" CYAN << DefinedCommands[request->id].name  << ATTROFF<< endl;
				shmPtr->clients.subscribe(a_client, request->id);
			} else {
				//if(verbose)
				cout << GREEN << me->name << ATTROFF " -- unsubscribing " YELLOW << a_client->name 
				     << ATTROFF ":"  CYAN << DefinedCommands[request->id].name << ATTROFF << endl;
				shmPtr->clients.unsubscribe(a_client, request->id);
			}

		} break;

		//----------------------------------------------------------
		// Now handle the storing of commands to local memory
		//----------------------------------------------------------
		case CMD_TELEM:
			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}
			//cout << "CMD_TELEM from "<< a_client->name << endl;

			CopyBufToStruct( dataPtr, a_client->telemetry);

			// update gps status
			a_client->state.gps = 1;
			shmPtr->clients.setRange(a_client);
			break;

		case CMD_GPS_TELEM: {
			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}

			//cout << "CMD_GPS_TELEM from "<< a_client->name << endl;
			CopyBufToStruct( dataPtr, a_client->telemetry.gps);

			a_client->state.gps = 1;

			if(shmPtr->clients.getClient((me->telemetry_client.id)) == a_client) {
				memcpy( &(me->telemetry.gps),&(a_client->telemetry.gps),sizeof(GPSTelem_t));
				me->state.gps = 1;
			}

			shmPtr->clients.setRange(a_client);

		} break;

		case CMD_START: {
			a_client->state.running = 1;
		} break;

		case CMD_STOP: {
			a_client->state.running = 0;
		} break;

		case CMD_GPS_LATLON: {
			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}

			//cout << "CMD_GPS_LATLON" << endl;
			GPSLatLon_t location;
			CopyBufToStruct( dataPtr, location);

			// only update GPS if not a UA with CMD_TELEM
			if( !a_client->capabilities.find(CMD_TELEM) ) {
				a_client->telemetry.gps.pos.lat = location.lat;
				a_client->telemetry.gps.pos.lon = location.lon;

				// update gps status
				a_client->state.gps = 1;
				shmPtr->clients.setRange(a_client);
			}
		} break;

		case CMD_SONDE: {
			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}

			Sonde_t sonde;
			CopyBufToStruct( dataPtr, sonde);
			//cout << "CMD_SONDE: from=" << a_client->name << " sonde=" << (int)sonde.id 
			//<< " press=" << sonde.pth.pressure << " temp=" << sonde.pth.temp << endl;
			if( sonde.id < MAX_NUM_SONDE ) {
				a_client->sonde[ sonde.id ] = sonde;
			} else
				cout << "CMD_SONDE: invalid id=" << (int)sonde.id << endl;
			
		} break;

		case CMD_WIND: {
			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}
			memcpy(&(a_client->wind),dataPtr,sizeof(WindData_t));
			
		} break;

		case CMD_ANT_PAS_TELEM: {
			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}

			CopyBufToStruct( dataPtr, a_client->antenna_pas);
			//cout << "CMD_ANT_PAS_TELEM: from=" << a_client->name << " azimuth=" << a_client->antenna_pas.azimuth  << " mode=" << (int)(a_client->antenna_pas.mode) << endl;;
			
		} break;

		case CMD_ANT_APS_TELEM: {
			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}

			CopyBufToStruct( dataPtr, a_client->antenna_aps);
			//cout << "CMD_ANT_APS_TELEM: from=" << a_client->name << " azimuth=" << a_client->antenna_aps.azimuth  << " mode=" << (int)(a_client->antenna_aps.cmdType) << endl;;
			
		} break;

		case CMD_LINK_STATUS: {
			if(!a_client) {
				cout << "Data_Distribution::parse - could not get a valid client for a stream req" << endl;
				break;
			}

			//cout << "CMD_LINK_STATUS" << endl;
			// since the rssi trace client already sets this, no need to rewrite value
			// of client. note this is probably bad since the vcgui should subscribe, but
			// the gatewaydaemon is the vcgui.
			if( a_client != me ) {
				int num_rssi_val = cmd.num()/sizeof(LinkStatus_t);
				//cout << getName() << "-- CMD_LINK_STATUS --" << endl
				//<< "\tMessage From: " << a_client->name << " Num Links=" << num_rssi_val << endl;
			
				LinkStatus_t * link  = (LinkStatus_t *)dataPtr;
				for( int ii=0; ii < num_rssi_val; ii++) {
					a_client->linkstatus[ii] = link[ii];
			
					//printf("\t%i: IP=%i.%i.%i.%i MAC=%02X:%02X:%02X:%02X:%02X:%02X last_rx=%f\n",
					//ii, link[ii]->ip[0], link[ii]->ip[1], link[ii]->ip[2], link[ii]->ip[3],
					//link[ii]->mac[0],link[ii]->mac[1],link[ii]->mac[2],link[ii]->mac[3],link[ii]->mac[4],link[ii]->mac[5],
					//link[ii]->last_rx);
				}
			}
		} break;

	} // end of command switch

	mutex_unlock();
}


